package br.usjt.mazerobot;

import br.usjt.mazerobot.commons.comunicacao.Encoder;
import br.usjt.mazerobot.commons.comunicacao.Pacote;
import br.usjt.mazerobot.commons.constante.Direcao;
import br.usjt.mazerobot.commons.constante.IdentificadorPacote;
import br.usjt.mazerobot.commons.constante.Status;
import br.usjt.mazerobot.commons.navegacao.Celula;
import br.usjt.mazerobot.commons.navegacao.Mapa;
import br.usjt.mazerobot.comunicacao.ComunicadorBluetooth;
import br.usjt.mazerobot.util.Logger;

public class Controle {

    //================== Variaveis de controle =================================
    private Robo robo;
    private Mapa mapa;
    private Encoder encoder;
    private Pacote pacote;
    //===================== Parametros gerais ==================================
    /**indica qual cor sera considerada objetivo*/
    private int OBJETIVO;
    //======================= Variaveis auxiliares =============================
    private Celula posicaoAtual;
    private Celula posicaoAnterior;
    private Direcao direcaoAtual;
    private TabelaResponsavel tabelaResponsavel = new TabelaResponsavel();
    private Celula[] listaTemporaria = new Celula[3];
    private boolean pausar = false;
    private boolean executar = true;
    private static boolean respostaVoltaParaInicio;
    private static final byte SIM = 1;
    private Logger logger;

    /**
     * Construtor
     * @param robo
     */
    public Controle(Robo robo) {

        this.encoder = new Encoder();
        this.robo = robo;
        this.pacote = new Pacote();
        this.logger = new Logger(robo.getComunicador());

    }

    public void start(){
        aguardaPacote.start();
        statusRobo.start();
    }

    //==================== Modo de Busca ======================================
    private void executarModoMapeamentoCompleto() {
        logger.info("Iniciando o modo mapeamento completo...");
        inicializaModoBusca();
        boolean objetivoEncontrado = false;

        while(executar){
            analisarCelulasAdjacentes();

            if (isPosicaoAtualInterseccao()) {
                atualizaStatusAtualNoMapa(Status.INTERSECCAO);
            } else {
                atualizaStatusAtualNoMapa(Status.PERCORRIDO);
            }

            if (existeDirecaoValida(Status.LIVRE)) {
                andarUmBloco();
                atualizaStatusAtualNoMapa(Status.PERCORRIDO);
                if(!objetivoEncontrado){
                    if (encontrouObjetivo()) {
                        atualizaStatusAtualNoMapa(Status.OBJETIVO);
                        objetivoEncontrado = true;
                    }
                }
            } else {
                //Se nao existem mais caminhos validos , encerra o modo de busca
                if (!volta()) {
                    pacote = new Pacote();
                    pacote.setCabecalho(IdentificadorPacote.AVISO_MAPEAMENTO_FINALIZADO.getCodigo());
                    robo.getComunicador().enviar(pacote);
                    executar = false;
                    logger.info("Fim, mapeamento completo");
                }
            }
        }
    }

    private void executarModoEncontrarObjetivo() {
        logger.info("Iniciando o modo encontrar objetivo...");
        inicializaModoBusca();

        while (executar) {

            analisarCelulasAdjacentes();

            if (isPosicaoAtualInterseccao()) {
                atualizaStatusAtualNoMapa(Status.INTERSECCAO);
            } else {
                atualizaStatusAtualNoMapa(Status.PERCORRIDO);
            }

            if (existeDirecaoValida(Status.LIVRE)) {
                andarUmBloco();
                atualizaStatusAtualNoMapa(Status.PERCORRIDO);
                if (encontrouObjetivo()) {
                    atualizaStatusAtualNoMapa(Status.OBJETIVO);
                     //Pergunta se deseja voltar para o inicio
                    if(confirmaVoltaParaInicio()){
                        logger.info("Voltando para o inicio");
                        voltaParaInicio();
                    }
                    executar = false;
                    pacote = new Pacote();
                    pacote.setCabecalho(IdentificadorPacote.AVISO_MAPEAMENTO_FINALIZADO.getCodigo());
                    robo.getComunicador().enviar(pacote);
                    logger.info("Fim, o objetivo foi encontrado");
                }
            } else {
                //Se nao existem mais caminhos validos , encerra o modo de busca
                if (!volta()) {
                    pacote = new Pacote();
                    pacote.setCabecalho(IdentificadorPacote.AVISO_OBJETIVO_NAO_ENCONTRADO.getCodigo());
                    robo.getComunicador().enviar(pacote);
                    executar = false;
                    logger.info("Fim, o objetivo nao foi encontrado");
                }
            }
        }
    }

    private void inicializaModoBusca() {

        executar = true;
        mapa = new Mapa();

        //Marca a posição anterior como vazio
        posicaoAnterior = new Celula((short) 0, (short) -1, Status.VAZIO.getCodigo());

        //Inicializa a posição atial como Percorrida , pois o robo já esta nela
        posicaoAtual = new Celula((short) 0, (short) 0, Status.PERCORRIDO.getCodigo());
        direcaoAtual = Direcao.NORTE;

        mapa.adiciona(posicaoAtual);
        mapa.adiciona(posicaoAnterior);
        enviaCelulaSistemaAuxiliar(posicaoAtual);
    }

    //===================== Controle Robo ======================================
    /**verifica se a distancia encontrada pelo robo é considerada um bloqueio*/
    private boolean existeBloqueio() {
        robo.ativarSensorUltrasonico();
        int distanciaEncontrada = robo.getDistanciaSensorUltrasonico();
        robo.desativarSensorUltrasonico();
        return distanciaEncontrada <= Configuracao.DISTANCIA_BLOQUEIO;
    }

    private boolean encontrouObjetivo() {
        robo.ativarSensorLuminosidade();
        int cor = robo.getLuminosidadeSensor();
        int tolerancia = 1;
        robo.desativarSensorLuminosidade();
        return (cor > (OBJETIVO - tolerancia) && cor < (OBJETIVO + tolerancia));
    }

    private void andarUmBloco() {
        if (executar) {
            while (pausar) {
                aguardar();
            }
            //faz uma copia dos valores da posicaoAtual para a posicaoAnterior
            posicaoAnterior.setX(posicaoAtual.getX());
            posicaoAnterior.setY(posicaoAtual.getY());
            posicaoAnterior.setStatus(posicaoAtual.getStatus());

            robo.andarFrente();
            posicaoAtual.setX((short) (posicaoAtual.getX() + direcaoAtual.getIncrementoX()));
            posicaoAtual.setY((short) (posicaoAtual.getY() + direcaoAtual.getIncrementoY()));
            posicaoAtual.setStatus(mapa.consulta(posicaoAtual));
        }
    }

    private void inverteCaminho() {
        atualizaStatusAtualNoMapa(Status.INCORRETO);
        robo.inverterSentido();
        direcaoAtual = getDirecaoInversa(direcaoAtual);
    }

    /**
     * Envia uma celula para o sistema auxiliar exibir na tela
     * @param celula
     */
    private void enviaCelulaSistemaAuxiliar(Celula celula) {
        pacote = encoder.codificar(celula);
        robo.getComunicador().enviar(pacote);
    }

    //===================== Controle Tabela Responsaveis =======================
    /** a coordenada passada como parametro é adicionada na tabelaResponsaveis como chave
    a posicao atual é considerada valor*/
    private void adicionarResponsavel(Celula coordenada) {
        tabelaResponsavel.adicionaResponsavel(coordenada, posicaoAtual);
    }

    /**verifica na tabelaResponsaveis se a coordenada é de responsabilidade da posicao atual*/
    private boolean isResponsavel(Celula coordenada) {
        Celula responsavel = tabelaResponsavel.consultaResponsavel(coordenada);
        return responsavel.equals(posicaoAtual);
    }

    //====================== Lista Temporaria ==================================
    /**preenche a lista temporaria com 3 coordenadas, <br>
     * indice 0 representa a coordenada a esquerda da posicao atual<br>
     * indice 1 representa a coordenada a frente da posicao atual<br>
     * indice 2 representa a coordenada a direita da posicao atual
     */
    private void preencheCoordenadasListaTemporaria() {

        Celula coordenadaEsquerda = new Celula();
        Celula coordenadaFrente = new Celula();
        Celula coordenadaDireita = new Celula();

        short x = posicaoAtual.getX();
        short y = posicaoAtual.getY();

        //representa celula a esquerda
        coordenadaEsquerda.setX((short) (x + getDirecaoEsquerda(direcaoAtual).getIncrementoX()));
        coordenadaEsquerda.setY((short) (y + getDirecaoEsquerda(direcaoAtual).getIncrementoY()));
        //representa celula a frente
        coordenadaFrente.setX((short) (x + direcaoAtual.getIncrementoX()));
        coordenadaFrente.setY((short) (y + direcaoAtual.getIncrementoY()));
        //representa celula a direita
        coordenadaDireita.setX((short) (x + getDirecaoDireita(direcaoAtual).getIncrementoX()));
        coordenadaDireita.setY((short) (y + getDirecaoDireita(direcaoAtual).getIncrementoY()));

        listaTemporaria[0] = coordenadaEsquerda;
        listaTemporaria[1] = coordenadaFrente;
        listaTemporaria[2] = coordenadaDireita;

    }

    /**preenche a lista temporaria com as coordenadas e status das celuas adjacentes(esquerda,frente,direita)*/
    private void analisarCelulasAdjacentes() {
        //encontra as coordenadas das celulas adjacentes*/
        preencheCoordenadasListaTemporaria();
        for (Celula celula : listaTemporaria) {
            //verifica se a celula não contem status
            if (mapa.consulta(celula) == Status.VAZIO.getCodigo()) {
                //listaTemporaria[1] representa a frente do robo
                if (celula == listaTemporaria[1]) {
                    robo.girarSensorUltrasonicoFrente();
                    //listaTemporaria[2] representa a direita do robo
                } else if (celula == listaTemporaria[2]) {
                    robo.girarSensorUltrasonicoDireita();
                }
                if (existeBloqueio()) {
                    celula.setStatus(Status.BLOQUEIO.getCodigo());
                } else {
                    //a posicao atual sera a responsavel por verificar a celula informada
                    adicionarResponsavel(celula);
                    celula.setStatus(Status.LIVRE.getCodigo());
                }
                mapa.adiciona(celula);
                enviaCelulaSistemaAuxiliar(celula);
            } else {
                //consulta o status da celula
                byte status = mapa.consulta(celula);
                if (status == Status.LIVRE.getCodigo()) {
                    if (isResponsavel(celula)) {
                        celula.setStatus(status);
                    } else {
                        //se a celula atual nao for responsavel pela celula que esta com o status livre
                        //ela é tratada como bloqueio
                        celula.setStatus(Status.BLOQUEIO.getCodigo());
                    }
                } else {
                    celula.setStatus(status);
                }
            }
        }
        //faz com que o sensor do robo volte para a posicao inicial
        robo.girarSensorUltrasonicoEsquerda();
    }

    private void consultaCelulaAdjacentes() {
        preencheCoordenadasListaTemporaria();
        for (Celula celula : listaTemporaria) {
            celula.setStatus(mapa.consulta(celula));
        }
    }

    //================= Altera Direção =========================================
    private Direcao getDirecaoDireita(Direcao direcao) {
        byte codigoDirecaoDireita = (byte) ((direcao.getCodigo() + 1) % 4);
        return Direcao.fromValue(codigoDirecaoDireita);
    }

    private Direcao getDirecaoEsquerda(Direcao direcao) {
        byte codigoDirecaoDireita = (byte) ((direcao.getCodigo() + 3) % 4);
        return Direcao.fromValue(codigoDirecaoDireita);
    }

    private Direcao getDirecaoInversa(Direcao direcao) {
        byte codigoDirecaoInversa = (byte) ((direcao.getCodigo() + 2) % 4);
        return Direcao.fromValue(codigoDirecaoInversa);
    }

    //===================== Analise Intersecção ================================
    /**verifica se a posicao atual é uma interseccao*/
    private boolean isPosicaoAtualInterseccao() {
        int qtdeCaminhosValidos = 0;
        //verifica o status das posicoes contidas na listaTemporaria
        for (Celula c : listaTemporaria) {
            if (c.getStatus() == Status.LIVRE.getCodigo() || c.getStatus() == Status.PERCORRIDO.getCodigo()
                    || c.getStatus() == Status.INTERSECCAO.getCodigo()) {
                qtdeCaminhosValidos++;
            }
        }
        short statusPosicaoAnterior = posicaoAnterior.getStatus();
        if (statusPosicaoAnterior == Status.LIVRE.getCodigo() || statusPosicaoAnterior == Status.PERCORRIDO.getCodigo()
                || statusPosicaoAnterior == Status.INTERSECCAO.getCodigo()) {
            qtdeCaminhosValidos++;
        }
        return qtdeCaminhosValidos > 2;
    }

    /**define qual caminho tem prioridade em uma interseccao*/
    private void motorDePrioridade() {

        if (existeDirecaoValida(Status.LIVRE)) {
            andarUmBloco();
            atualizaStatusAtualNoMapa(Status.PERCORRIDO);
        } else if (existeDirecaoValida(Status.PERCORRIDO)) {
            andarUmBloco();
            atualizaStatusAtualNoMapa(Status.INCORRETO);
        } else if (existeDirecaoValida(Status.INTERSECCAO)) {
            andarUmBloco();
        }
    }

    //=================== Atualização ==========================================
    private void atualizaStatusAtualNoMapa(Status novoStatus) {

        if(Status.OBJETIVO.getCodigo() != mapa.consulta(posicaoAtual) ) {

            posicaoAtual.setStatus(novoStatus.getCodigo());
            mapa.adiciona(posicaoAtual);

            //Envia as informacoes para o projeto que esta no PC
            if(Thread.currentThread().getName().equals("mapeamentoCompleto") && novoStatus.equals(Status.INCORRETO)){
                Celula celula = new Celula(posicaoAtual.getX(), posicaoAtual.getY());
                celula.setStatus(Status.PERCORRIDO.getCodigo());
                enviaCelulaSistemaAuxiliar(celula);
            }else{
                enviaCelulaSistemaAuxiliar(posicaoAtual);
            }
            logger.info("Enviando status da posicao atual: " +  novoStatus.name());
        }
    }

    //=================== Caminho de volta ====================================
    /**
     * @return true se ainda existem caminhos validos,
     * retorna false se nao existem mais caminhos validos
     *
     */
    private boolean volta() {
        if (posicaoAtual.getStatus() == Status.INTERSECCAO.getCodigo()) {
            motorDePrioridade();
            return true;
        } else {
            if (existeDirecaoValida(Status.PERCORRIDO, Status.INTERSECCAO)) {
                atualizaStatusAtualNoMapa(Status.INCORRETO);
                andarUmBloco();
                return true;
            } else {
                inverteCaminho();
                if (posicaoAnterior.getStatus() == Status.INCORRETO.getCodigo()) {
                    return false;
                } else {
                    analisarCelulasAdjacentes();
                    if (mapa.consulta(posicaoAnterior) != Status.BLOQUEIO.getCodigo()) {
                        andarUmBloco();
                        return true;
                    }
                    return false;
                }
            }
        }
    }

    private boolean confirmaVoltaParaInicio(){
        Pacote pacote = new Pacote();
        pacote.setCabecalho(IdentificadorPacote.CONFIRMA_VOLTA_PARA_INICIO.getCodigo());
        robo.getComunicador().enviar(pacote);
        pausar();
        while(pausar){
            aguardar();
        }
        return respostaVoltaParaInicio;
    }


    /**
     * Volta para a posição x = 0, y =0 consultando somente os dados que ja estao gravados no mapa
     * que foi preenchido até encontrar o objetivo
     */
    private void voltaParaInicio() {
        robo.inverterSentido();
        direcaoAtual = getDirecaoInversa(direcaoAtual);
        consultaCelulaAdjacentes();
        while (existeDirecaoValida(Status.PERCORRIDO, Status.INTERSECCAO)) {
            andarUmBloco();
            atualizaStatusAtualNoMapa(Status.PERCORRIDO);
            consultaCelulaAdjacentes();
        }
    }

    /**verifica se existe um caminho com os statusValidos passado como parametro, se existir ele posiciona o robo na
     * primeira direcao valida
     * @param statusValidos
     * @return
     */
    private boolean existeDirecaoValida(Status... statusValidos) {

        for (Celula celula : listaTemporaria) {
            for (Status statusValido : statusValidos) {
                if (celula.getStatus() == statusValido.getCodigo()) {
                    //posicao à esquerda do robo
                    if (celula == listaTemporaria[0]) {
                        robo.girarEsquerda();
                        logger.info("Girou esquerda");
                        direcaoAtual = getDirecaoEsquerda(direcaoAtual);
                    } //posicao à direita do robo
                    else if (celula == listaTemporaria[2]) {
                        robo.girarDireita();
                        logger.info("Girou direita");
                        direcaoAtual = getDirecaoDireita(direcaoAtual);
                    }
                    return true;
                }
            }
        }
        //nenhum caminho é valido
        return false;
    }

    //====================== Auxiliar ==========================================
    private void aguardar() {
        try {
            Thread.sleep(500);
        } catch (InterruptedException ex) {
            robo.getComunicador().enviarLog(ex.getMessage());
        }
    }
    //================= Gerenciamento de pacote ================================
    private Thread aguardaPacote = new Thread(new Runnable() {

        @Override
        public void run() {
            while (!Thread.currentThread().isInterrupted()) {
                try {
                    Pacote pacote = robo.getComunicador().receber();
                    direcionaPacote(pacote);
                } catch (Exception e) {
                    Thread.currentThread().interrupt();
                }
            }
        }
    });

    private void direcionaPacote(Pacote pacote) {

        if (pacote != null) {
            IdentificadorPacote id = IdentificadorPacote.fromValue(pacote.getCabecalho());
            logger.info("Recebeu o pacote: " + id.name());
            switch (id) {
                case MODO_DE_OPERACAO_ENCONTRAR_OBJETIVO:
                    this.OBJETIVO = encoder.decodificaObjetivo(pacote);
                    if (!existeThreadAtiva()) {
                        modoDeExecucao =  new Thread(encontrarObjetivo);
                        modoDeExecucao.setName("encontrarObjetivo");
                        modoDeExecucao.start();
                    }
                    break;

                case MODO_DE_OPERACAO_MAPEAMENTO_COMPLETO:
                    this.OBJETIVO = encoder.decodificaObjetivo(pacote);
                    if (!existeThreadAtiva()) {
                        modoDeExecucao =  new Thread(mapeamentoCompleto);
                        modoDeExecucao.setName("mapeamentoCompleto");
                        modoDeExecucao.start();
                    }
                    break;

                case PAUSAR_EXECUCAO:
                    pausar();
                    break;

                case CONTINUAR_EXECUCAO:
                    continuar();
                    break;

                case ENCERRAR_MODO_DE_OPERACAO:
                    encerrarModoDeOperacao();
                    break;

                case CONFIRMA_VOLTA_PARA_INICIO:
                    respostaVoltaParaInicio = pacote.getConteudo()[0]== SIM;
                    continuar();
                    break;
            }
        }
    }

    /**Para o robo apos seu ultimo movimento , só volta a funcionar quando o metodo continuar() for executado*/
    private void pausar() {
        pausar = true;
        logger.info("Aguardando... o pause foi ativado");
    }

    /**Faz com que o robo de continuidade na tarefa que estava executando*/
    private void continuar() {
        pausar = false;
        logger.info("Retornando a execucao... o pause foi desativado");
    }

    private void encerrarModoDeOperacao() {
        executar = false;
    }
    private Thread encontrarObjetivo = new Thread(new Runnable() {

        @Override
        public void run() {
            executarModoEncontrarObjetivo();
        }
    });
    private Thread mapeamentoCompleto = new Thread(new Runnable() {

        @Override
        public void run() {
            executarModoMapeamentoCompleto();
        }
    });

    private Thread modoDeExecucao = new Thread();

    private boolean existeThreadAtiva(){
        return modoDeExecucao.isAlive();
    }

        private Thread statusRobo = new Thread(new Runnable() {

        public void run() {

            while (!Thread.currentThread().isInterrupted()) {
                try {
                    if (robo.getComunicador() instanceof ComunicadorBluetooth) {
                        int intensidadeSinalBluetooth = ((ComunicadorBluetooth) robo.getComunicador()).getIntensidadeSinalBluetooth();
                        Pacote pacote = encoder.codificar(IdentificadorPacote.SINAL_BLUETOOTH, intensidadeSinalBluetooth);
                        robo.getComunicador().enviar(pacote);

                        Thread.sleep(1000);
                    }
                } catch (Exception e) {
                    Thread.currentThread().interrupt();
                }
            }
        }
    });

}